#include "Quaternions.h"
#include "Vector3.h"
#include "Angles.h"
#include "Rotation.h"
#include "Math.h"
#include "Matrix4.h"

namespace Core
{
	inline rtQuaternions::rtQuaternions(void) 
	{
	}

	inline rtQuaternions::rtQuaternions(float x, float y, float z, float w) 
	{
		this->x = x;
		this->y = y;
		this->z = z;
		this->w = w;
	}

	inline float rtQuaternions::operator [] (int index) const 
	{
		assert((index >= 0) && (index < 4));
		return (&x)[index];
	}

	inline float& rtQuaternions::operator [] (int index) 
	{
		assert((index >= 0) && (index < 4));
		return (&x)[index];
	}

	inline rtQuaternions rtQuaternions::operator - () const 
	{
		return rtQuaternions(-x, -y, -z, -w);
	}

	inline rtQuaternions &rtQuaternions::operator = (const rtQuaternions& a) 
	{
		x = a.x;
		y = a.y;
		z = a.z;
		w = a.w;

		return *this;
	}

	inline rtQuaternions rtQuaternions::operator + (const rtQuaternions& a) const 
	{
		return rtQuaternions(x + a.x, y + a.y, z + a.z, w + a.w);
	}

	inline rtQuaternions& rtQuaternions::operator += (const rtQuaternions& a) 
	{
		x += a.x;
		y += a.y;
		z += a.z;
		w += a.w;

		return *this;
	}

	inline rtQuaternions rtQuaternions::operator - (const rtQuaternions& a) const 
	{
		return rtQuaternions(x - a.x, y - a.y, z - a.z, w - a.w);
	}

	inline rtQuaternions& rtQuaternions::operator-=(const rtQuaternions& a) 
	{
		x -= a.x;
		y -= a.y;
		z -= a.z;
		w -= a.w;

		return *this;
	}

	inline rtQuaternions rtQuaternions::operator * (const rtQuaternions& a) const 
	{
		return rtQuaternions(w*a.x + x*a.w + y*a.z - z*a.y,
			w*a.y + y*a.w + z*a.x - x*a.z,
			w*a.z + z*a.w + x*a.y - y*a.x,
			w*a.w - x*a.x - y*a.y - z*a.z);
	}

	inline rtVector3 rtQuaternions::operator * (const rtVector3& a) const 
	{
		float xxzz = x*x - z*z;
		float wwyy = w*w - y*y;

		float xw2 = x*w*2.0f;
		float xy2 = x*y*2.0f;
		float xz2 = x*z*2.0f;
		float yw2 = y*w*2.0f;
		float yz2 = y*z*2.0f;
		float zw2 = z*w*2.0f;

		return rtVector3((xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
			(xy2 - zw2)*a.x			+ (y*y+w*w-x*x-z*z)*a.y	+ (yz2 + xw2)*a.z,
			(xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z);
	}

	inline rtQuaternions rtQuaternions::operator*(float a) const 
	{
		return rtQuaternions(x * a, y * a, z * a, w * a);
	}

	inline rtQuaternions operator*(const float a, const rtQuaternions& b) 
	{
		return b * a;
	}

	inline rtVector3 operator * (const rtVector3& a, const rtQuaternions& b) 
	{
		return b * a;
	}

	inline rtQuaternions& rtQuaternions::operator *= (const rtQuaternions& a) 
	{
		*this = *this * a;

		return *this;
	}

	inline rtQuaternions& rtQuaternions::operator *= (float a) 
	{
		x *= a;
		y *= a;
		z *= a;
		w *= a;

		return *this;
	}

	inline bool rtQuaternions::compare(const rtQuaternions& a) const 
	{
		return ((x == a.x) && (y == a.y) && (z == a.z) && (w == a.w));
	}

	inline bool rtQuaternions::compare(const rtQuaternions& a, const float epsilon) const 
	{
		if (rtMath::Fabs(x - a.x) > epsilon) 
		{
			return false;
		}
		if (rtMath::Fabs(y - a.y) > epsilon) 
		{
			return false;
		}
		if (rtMath::Fabs(z - a.z) > epsilon) 
		{
			return false;
		}
		if (rtMath::Fabs(w - a.w) > epsilon) 
		{
			return false;
		}
		return true;
	}

	inline bool rtQuaternions::operator == (const rtQuaternions& a) const 
	{
		return compare(a);
	}

	inline bool rtQuaternions::operator != (const rtQuaternions& a) const 
	{
		return !compare(a);
	}

	inline void rtQuaternions::set(float x, float y, float z, float w) 
	{
		this->x = x;
		this->y = y;
		this->z = z;
		this->w = w;
	}

	inline rtQuaternions rtQuaternions::inverse(void) const 
	{
		return rtQuaternions(-x, -y, -z, w);
	}

	inline float rtQuaternions::length(void) const 
	{
		float len;

		len = x * x + y * y + z * z + w * w;
		return rtMath::Sqrt(len);
	}

	inline rtQuaternions& rtQuaternions::normalize(void) 
	{
		float len;
		float ilength;

		len = this->length();
		if (len) 
		{
			ilength = 1 / len;
			x *= ilength;
			y *= ilength;
			z *= ilength;
			w *= ilength;
		}
		return *this;
	}

	inline float rtQuaternions::calcW(void) const 
	{
		return sqrt(fabs(1.0f - (x * x + y * y + z * z)));
	}

	inline int rtQuaternions::getDimension(void) const 
	{
		return 4;
	}

	inline const float* rtQuaternions::toFloatPtr(void) const 
	{
		return &x;
	}

	inline float* rtQuaternions::toFloatPtr(void) 
	{
		return &x;
	}

	rtAngles rtQuaternions::toAngles(void) const 
	{
		return toMat3().toAngles();
	}


	rtRotation rtQuaternions::toRotation(void) const 
	{
		rtVector3 vec;
		float angle;

		vec.x = x;
		vec.y = y;
		vec.z = z;
		angle = rtMath::ACos(w);
		if (angle == 0.0f) 
		{
			vec.set(0.0f, 0.0f, 1.0f);
		} 
		else 
		{
			vec.normalize();
			vec.fixDegenerateNormal();
			angle *= 2.0f * rtMath::M_RAD2DEG;
		}
		return rtRotation(rtVector3::vec3_origin, vec, angle);
	}


	rtMatrix3 rtQuaternions::toMat3(void) const 
	{
		rtMatrix3	mat;
		float	wx, wy, wz;
		float	xx, yy, yz;
		float	xy, xz, zz;
		float	x2, y2, z2;

		x2 = x + x;
		y2 = y + y;
		z2 = z + z;

		xx = x * x2;
		xy = x * y2;
		xz = x * z2;

		yy = y * y2;
		yz = y * z2;
		zz = z * z2;

		wx = w * x2;
		wy = w * y2;
		wz = w * z2;

		mat[0][0] = 1.0f - (yy + zz);
		mat[0][1] = xy - wz;
		mat[0][2] = xz + wy;

		mat[1][0] = xy + wz;
		mat[1][1] = 1.0f - (xx + zz);
		mat[1][2] = yz - wx;

		mat[2][0] = xz - wy;
		mat[2][1] = yz + wx;
		mat[2][2] = 1.0f - (xx + yy);

		return mat;
	}


	rtMatrix4 rtQuaternions::toMat4(void) const 
	{
		return toMat3().toMat4();
	}



	rtVector3 rtQuaternions::toAngularVelocity(void) const 
	{
		rtVector3 vec;

		vec.x = x;
		vec.y = y;
		vec.z = z;
		vec.normalize();
		return vec * rtMath::ACos(w);
	}



	rtQuaternions &rtQuaternions::slerp(const rtQuaternions &from, const rtQuaternions &to, float t) 
	{
		rtQuaternions	temp;
		float	omega, cosom, sinom, scale0, scale1;

		if (t <= 0.0f) 
		{
			*this = from;
			return *this;
		}

		if (t >= 1.0f) 
		{
			*this = to;
			return *this;
		}

		if (from == to) 
		{
			*this = to;
			return *this;
		}

		cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
		if (cosom < 0.0f) 
		{
			temp = -to;
			cosom = -cosom;
		} 
		else 
		{
			temp = to;
		}

		if ((1.0f - cosom) > 1e-6f) 
		{

			scale0 = 1.0f - cosom * cosom;
			sinom = rtMath::InvSqrt(scale0);
			omega = rtMath::ATan16(scale0 * sinom, cosom);
			scale0 = rtMath::Sin16((1.0f - t) * omega) * sinom;
			scale1 = rtMath::Sin16(t * omega) * sinom;
		}
		else 
		{
			scale0 = 1.0f - t;
			scale1 = t;
		}

		*this = (scale0 * from) + (scale1 * temp);
		return *this;
	}

}


